// Have include guard.
#ifndef TWOJOYSTICKDRIVESERVICE_H
#define TWOJOYSTICKDRIVESERVICE_H

// Include files.
#include <vector>
#include "WPILib.h"
#include "../Values/TwoJoystickValue.hpp"
#include "../Filters/CompoundFilter.hpp"
#include "Service.hpp"

/**
 * The TwoJoystickDriveController drives using input from two joysticks.
 * 
 * @tparam Drive Needs to have a function called Stop that takes no arguments.
 * @tparam Handler
 */
template
<
	typename Drive,
	typename Value,
	void(*Handler)(Drive &drive, const Value &value)
>
class TwoJoystickDriveService :
	public Service
{
	public:
		typedef TwoJoystickValue Input;
		typedef Value Output;
		typedef Filter<Input, Output> Filter;
	protected:
		Drive &m_drive;
		CompoundFilter<Input, Output> m_filter;
		GenericHID &m_joystickLeft;
		GenericHID &m_joystickRight;
	public:
		TwoJoystickDriveService(Drive &drive, boost::shared_ptr<Filter> driveMode, GenericHID &leftJoystick, GenericHID &rightJoystick) :
			m_drive(RequireControl(drive)),
			m_filter(driveMode),
			m_joystickLeft(leftJoystick),
			m_joystickRight(rightJoystick)
		{
		}
		
		TwoJoystickDriveService(Drive &drive, Filter *driveMode, GenericHID &leftJoystick, GenericHID &rightJoystick, bool canDeleteDriveMode = false) :
			m_drive(RequireControl(drive)),
			m_filter(driveMode, canDeleteDriveMode),
			m_joystickLeft(leftJoystick),
			m_joystickRight(rightJoystick)
		{
		}
		
		template<typename T>
		void AddFilter(T *filter, bool canDeleteFilter)
		{
			m_filter.AddFilter(filter, canDeleteFilter);
		}
		
		template<typename T>
		void AddFilter(T filter)
		{
			m_filter.AddFilter(filter);
		}
	protected:
		virtual void HandlePeriodic()
		{
			Input input(m_joystickLeft, m_joystickRight);
			Output output = m_filter.Apply(input);
			Handler(m_drive, output);
		}
		
		virtual void HandleStop()
		{
			m_drive.Stop();
		}
};

#endif // #ifndef TWOJOYSTICKDRIVESERVICE_H 
